
#include <cassert>
#include <cmath>

#include "Math\Quaternion.h"


Quaternion::Quaternion()
    : m_quaternion(Vector4(0, 0, 0, 0))
{
}


Quaternion::Quaternion(const Vector4& vector)
    : m_quaternion(vector)
{}


Quaternion::Quaternion(FLOAT a, FLOAT b, FLOAT c, FLOAT d)
    : m_quaternion(Vector4(b, c, d, a))
{}


Quaternion::~Quaternion()
{
}


Quaternion::Quaternion(const Quaternion& other)
    : m_quaternion(other.m_quaternion)
{}



Quaternion& Quaternion::operator = (const Quaternion& other)
{
	// Self-assignement.
    if(this == &other)
        return *this;

	// Copy.
    m_quaternion = other.m_quaternion;

    return *this;
}


Quaternion Quaternion::CreateRotation(FLOAT x, FLOAT y, FLOAT z, FLOAT angle)
{
	// Impossible to rotate around null vector.
	assert(x != 0 || y != 0 || z != 0);

	// Create rotation.
    FLOAT half_angle = angle / 2;

    FLOAT sin = std::sin(half_angle);
    FLOAT cos = std::cos(half_angle);

	Vector4 result(x * sin, y * sin, z * sin, cos);

    return result.IsNull() ? Quaternion(result) : Quaternion(result).Normalize();
}


Quaternion Quaternion::CreateRotation(const Vector4& axis, FLOAT angle)
{
    return CreateRotation(axis.GetX(), axis.GetY(), axis.GetZ(), angle);
}


Vector4 Quaternion::GetVector() const
{
	Vector4 result = m_quaternion;
	result.SetW(0);	// To vector.

	return result;
}


FLOAT Quaternion::GetScalar() const
{
	return m_quaternion.GetW();
}


Quaternion Quaternion::Normalize() const
{
	FLOAT length = sqrt(pow(m_quaternion.GetX(), 2) +
						pow(m_quaternion.GetY(), 2) +
						pow(m_quaternion.GetZ(), 2) +
						pow(m_quaternion.GetW(), 2));

	assert(length != 0);

    return Quaternion(m_quaternion.GetX() / length,
				      m_quaternion.GetY() / length,
					  m_quaternion.GetZ() / length,
					  m_quaternion.GetW() / length);
}


Quaternion Quaternion::Interpolate(const Quaternion& other, FLOAT u) const
{
    // The parametric variable has to be in range [0, 1].
    assert(u >= 0 && u <= 1);

    const Quaternion& q1 = *this;
    const Quaternion& q2 = other;
    FLOAT dw = other.m_quaternion.GetW() - m_quaternion.GetW();

    return ((std::sin((1 - u) * dw) * q1) + (std::sin(u * dw) * q2)) * (1 / std::sin(dw));
}


Quaternion Quaternion::Add(const Quaternion& other) const
{
    return Quaternion(m_quaternion + other.m_quaternion);
}


Quaternion Quaternion::operator + (const Quaternion& other) const
{
    return Add(other);
}


Quaternion& Quaternion::operator += (const Quaternion& other)
{
	m_quaternion += other.m_quaternion;

	return *this;
}


Quaternion Quaternion::Sub(const Quaternion& other) const
{
    return Quaternion(m_quaternion - other.m_quaternion);
}


Quaternion Quaternion::operator -(const Quaternion& other) const
{
    return Sub(other);
}


Quaternion& Quaternion::operator -= (const Quaternion& other)
{
	m_quaternion -= other.m_quaternion;

	return *this;
}


Quaternion Quaternion::Mul(FLOAT factor) const
{
    return Quaternion(m_quaternion * factor);
}


FLOAT Quaternion::Dot(const Quaternion& other) const
{
    return m_quaternion.GetX() * other.m_quaternion.GetX() +
           m_quaternion.GetY() * other.m_quaternion.GetY() +
           m_quaternion.GetZ() * other.m_quaternion.GetZ() +
           m_quaternion.GetW() * other.m_quaternion.GetW();
}


Quaternion::operator Matrix4x4() const
{
    Matrix4x4 result = Matrix4x4::CreateIdentity();

    FLOAT diagonal = 1 - 2 * std::pow(m_quaternion.GetY(), 2) - 2 * std::pow(m_quaternion.GetZ(), 2);

    result(0, 0) = diagonal;
    result(0, 1) = 2 * m_quaternion.GetX() * m_quaternion.GetY() - 2 * m_quaternion.GetZ() * m_quaternion.GetW();
    result(0, 2) = 2 * m_quaternion.GetX() * m_quaternion.GetZ() + 2 * m_quaternion.GetY() * m_quaternion.GetW();
    result(1, 0) = 2 * m_quaternion.GetX() * m_quaternion.GetY() + 2 * m_quaternion.GetZ() * m_quaternion.GetW();
    result(1, 1) = diagonal;
    result(1, 2) = 2 * m_quaternion.GetY() * m_quaternion.GetZ() - 2 * m_quaternion.GetX() * m_quaternion.GetW();
    result(2, 0) = 2 * m_quaternion.GetX() * m_quaternion.GetZ() - 2 * m_quaternion.GetY() * m_quaternion.GetW();
    result(2, 1) = 2 * m_quaternion.GetY() * m_quaternion.GetZ() + 2 * m_quaternion.GetX() * m_quaternion.GetW();
    result(2, 2) = diagonal;

	return result;
}


Quaternion::operator D3DXMATRIX() const
{
    FLOAT diagonal = 1 - 2 * std::pow(m_quaternion.GetY(), 2) - 2 * std::pow(m_quaternion.GetZ(), 2);

	return D3DXMATRIX(diagonal,
					  2 * m_quaternion.GetX() * m_quaternion.GetY() + 2 * m_quaternion.GetZ() * m_quaternion.GetW(),
				      2 * m_quaternion.GetX() * m_quaternion.GetZ() - 2 * m_quaternion.GetY() * m_quaternion.GetW(),
					  0,
					  2 * m_quaternion.GetX() * m_quaternion.GetY() - 2 * m_quaternion.GetZ() * m_quaternion.GetW(),
					  diagonal,
					  2 * m_quaternion.GetY() * m_quaternion.GetZ() + 2 * m_quaternion.GetX() * m_quaternion.GetW(),
					  0,
					  2 * m_quaternion.GetX() * m_quaternion.GetZ() + 2 * m_quaternion.GetY() * m_quaternion.GetW(),
					  2 * m_quaternion.GetY() * m_quaternion.GetZ() - 2 * m_quaternion.GetX() * m_quaternion.GetW(),
					  diagonal,
					  0,
					  0,
					  0,
					  0,
					  1);
}


Quaternion operator * (const Quaternion& quaternion, FLOAT factor)
{
    return quaternion.Mul(factor);
}


Quaternion operator * (FLOAT factor, const Quaternion& quaternion)
{
    return quaternion.Mul(factor);
}